/**
 *
 */
package jp.seraph.jsade.model.nao;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import org.apache.commons.math.geometry.Vector3D;

import jp.seraph.jsade.core.DefaultPlayer;
import jp.seraph.jsade.core.Player;
import jp.seraph.jsade.effector.EffectorBuilder;
import jp.seraph.jsade.effector.NaoEffectorBuilder;
import jp.seraph.jsade.math.Point3D;
import jp.seraph.jsade.message.MessageUtil;
import jp.seraph.jsade.model.DefaultForceResistance;
import jp.seraph.jsade.model.DefaultGyroValue;
import jp.seraph.jsade.model.DefaultObjectCollection;
import jp.seraph.jsade.model.ForceResistance;
import jp.seraph.jsade.model.GyroValue;
import jp.seraph.jsade.model.Joint;
import jp.seraph.jsade.model.JointIdentifier;
import jp.seraph.jsade.model.ModelManager;
import jp.seraph.jsade.model.ModelObject;
import jp.seraph.jsade.model.ModelObjectIdentifier;
import jp.seraph.jsade.model.ModelPart;
import jp.seraph.jsade.perceptor.ForceResistancePerceptor;
import jp.seraph.jsade.perceptor.GyroPerceptor;
import jp.seraph.jsade.perceptor.HingeJointPerceptor;

/**
 * Nao用Manager実装。 とりあえず、あまり実行コストは考えていない。 速度が必要なら考える
 */
public class NaoModelManager implements ModelManager {
    public NaoModelManager(int aRobotID) {
        mRobotID = aRobotID;
        mEntityCollection = new DefaultObjectCollection();
        mCurrentGyroValue = new DefaultGyroValue(0, 0, 0);
        mTotalGyroValue = new DefaultGyroValue(0, 0, 0);
        mRightForceResistance = new DefaultForceResistance();
        mLeftForceResistance = new DefaultForceResistance();
        NaoObjectContainer tContainer = new NaoObjectContainer();
        Iterator<NaoJointIdentifier> tJointIterator = NaoJointIdentifier.getIterator();
        while (tJointIterator.hasNext())
            mEntityCollection.add(tContainer.getJoint(tJointIterator.next()));

        Iterator<NaoObjectIdentifier> tObjectIterator = NaoObjectIdentifier.getIterator();
        while (tObjectIterator.hasNext())
            mEntityCollection.add(tContainer.getObject(tObjectIterator.next()));
    }

    private int mRobotID;
    private DefaultObjectCollection mEntityCollection;
    private GyroValue mCurrentGyroValue;
    private GyroValue mTotalGyroValue;
    private ForceResistance mRightForceResistance;
    private ForceResistance mLeftForceResistance;
    private static final String LEFT_FOOT_RESISTANCE = "lf";
    private static final String RIGHT_FOOT_RESISTANCE = "rf";

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#getGyroValue()
     */

    public GyroValue getGyroValue() {
        return mCurrentGyroValue;
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#getLeftFootForceResistance()
     */

    public ForceResistance getLeftFootForceResistance() {
        return mLeftForceResistance;
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#setUpTurn()
     */
    public void setUpTurn() {
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#tearDownTurn()
     */
    public void tearDownTurn() {
        mLeftForceResistance = new DefaultForceResistance(Point3D.origin, Vector3D.zero);
        mRightForceResistance = new DefaultForceResistance(Point3D.origin, Vector3D.zero);
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#getRightFootForceResistance()
     */

    public ForceResistance getRightFootForceResistance() {
        return mRightForceResistance;
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#getTotalGyroValue()
     */

    public GyroValue getTotalGyroValue() {
        return mTotalGyroValue;
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#createEffectorBuilder()
     */
    public EffectorBuilder createEffectorBuilder() {
        return new NaoEffectorBuilder();
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#createPlayer()
     */
    public Player createPlayer() {
        return new DefaultPlayer(this.createEffectorBuilder(), this);
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#getObject(jp.seraph.jsade.model.ModelObjectIdentifier)
     */

    public ModelPart getObject(ModelObjectIdentifier aIdentifier) {
        if (aIdentifier instanceof NaoObjectIdentifier)
            return (ModelPart) this.getObject(aIdentifier.toObjectName());
        else
            throw new IllegalArgumentException(MessageUtil.oidIsNotNaoOid());
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#getAllObjects()
     */
    public List<ModelPart> getAllObjects() {
        List<ModelPart> tResult = new ArrayList<ModelPart>();
        for (ModelObject tObject : this) {
            if (tObject instanceof ModelPart) {
                ModelPart tParts = (ModelPart) tObject;
                tResult.add(tParts);
            }
        }

        return tResult;
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#getJoint(jp.seraph.jsade.model.JointIdentifier)
     */

    public Joint getJoint(JointIdentifier aIdentifier) {
        if (aIdentifier instanceof NaoJointIdentifier)
            return (Joint) this.getObject(aIdentifier.toJointName());
        else
            throw new IllegalArgumentException(MessageUtil.jidIsNotNaoJid());
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#getAllJoints()
     */
    public List<Joint> getAllJoints() {
        List<Joint> tResult = new ArrayList<Joint>();
        for (ModelObject tObject : this) {
            if (tObject instanceof Joint) {
                Joint tJoint = (Joint) tObject;
                tResult.add(tJoint);
            }
        }

        return tResult;
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#modifyModel(jp.seraph.jsade.perceptor.ForceResistancePerceptor)
     */
    public void modifyModel(ForceResistancePerceptor aFRP) {
        String tName = aFRP.getName();
        if (LEFT_FOOT_RESISTANCE.equals(tName)) {
            mLeftForceResistance = aFRP.getForceResistance();
        } else if (RIGHT_FOOT_RESISTANCE.equals(tName)) {
            mRightForceResistance = aFRP.getForceResistance();
        } else {
            throw new IllegalArgumentException(MessageUtil.unsupportedFPR(tName));
        }
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#modifyModel(jp.seraph.jsade.perceptor.GyroPerceptor)
     */
    public void modifyModel(GyroPerceptor aGyro) {
        mCurrentGyroValue = new DefaultGyroValue(aGyro);
        mTotalGyroValue = mTotalGyroValue.add(mCurrentGyroValue);
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelManager#modifyModel(jp.seraph.jsade.perceptor.HingeJointPerceptor)
     */
    public void modifyModel(HingeJointPerceptor aHingeJoint) {
        Joint tTargetJoint = this.getJoint(NaoJointIdentifier.getJointIdentifier(aHingeJoint.getName()));
        tTargetJoint.setAngle(aHingeJoint.getValue());
    }

    /**
     *
     * @see jp.seraph.jsade.model.ObjectCollection#getObject(int)
     */

    public ModelObject getObject(int aIndex) {
        return mEntityCollection.getObject(aIndex);
    }

    /**
     *
     * @see jp.seraph.jsade.model.ObjectCollection#getObject(java.lang.String)
     */

    public ModelObject getObject(String aName) {
        return mEntityCollection.getObject(aName);
    }

    /**
     *
     * @see jp.seraph.jsade.model.ObjectCollection#getObjectCount()
     */

    public int getObjectCount() {
        return mEntityCollection.getObjectCount();
    }

    /**
     *
     * @see java.lang.Iterable#iterator()
     */

    public Iterator<ModelObject> iterator() {
        return mEntityCollection.iterator();
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelObject#getAbsoluteCoordinate()
     */

    public Point3D getAbsoluteCoordinate() {
        return mEntityCollection.getAbsoluteCoordinate();
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelObject#getCentorOfGravity()
     */

    public Point3D getCentorOfGravity() {
        return mEntityCollection.getCentorOfGravity();
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelObject#getMass()
     */

    public double getMass() {
        return mEntityCollection.getMass();
    }

    /**
     *
     * @see jp.seraph.jsade.model.ModelObject#getName()
     */

    public String getName() {
        return Integer.toString(mRobotID);
    }
}
